#include "vector.h"
#include "math.h"

void mds_vector_add( mds_vector *lhs, const mds_vector *rhs )
{
	assert( lhs );
	assert( rhs );
	lhs->x += rhs->x;
	lhs->y += rhs->y;
	lhs->z += rhs->z;
}

void mds_vector_sub( mds_vector *lhs, const mds_vector *rhs )
{
	assert( lhs );
	assert( rhs );
	lhs->x -= rhs->x;
	lhs->y -= rhs->y;
	lhs->z -= rhs->z;
}

void mds_vector_vec( mds_vector *tgt, const mds_vector *from, const mds_vector *to )
{
	assert( tgt );
	assert( from );
	assert( to );
	
	tgt->x = to->x - from->x;
	tgt->y = to->y - from->y;
	tgt->z = to->z - from->z;
	
	
}

void mds_vector_ldz( mds_vector *tgt )
{
	assert( tgt );
	tgt->x = 0;
	tgt->y = 0;
	tgt->z = 0;
	
}

void mds_vector_mul( mds_vector *tgt, const Real u )
{
	assert( tgt );
	tgt->x *= u;
	tgt->y *= u;
	tgt->z *= u;
}

void mds_vector_mulset( mds_vector *lhs, const Real u, const mds_vector *rhs )
{
	assert( lhs );
	assert( rhs );
	lhs->x = u * rhs->x;
	lhs->y = u * rhs->y;
	lhs->z = u * rhs->z;
}

void mds_vector_muladd( mds_vector *lhs, const Real u, const mds_vector *rhs )
{
	assert( lhs );
	assert( rhs );
	lhs->x += u * rhs->x;
	lhs->y += u * rhs->y;
	lhs->z += u * rhs->z;
}

Real mds_vector_normsq( const mds_vector *tgt )
{
	assert( tgt );
	
	const Real x = tgt->x;
	const Real y = tgt->y;
	Real       n = x*x;
	const Real z = tgt->z;
	n += y*y;
	return n + z*z;
}

Real mds_vector_length( const mds_vector *tgt )
{
	return (Real)sqrt( mds_vector_normsq(tgt) );
}


Real mds_vector_dot( const mds_vector *lhs, const mds_vector *rhs )
{
	assert( lhs );
	assert( rhs );
	
	return (lhs->x * rhs->x) + (lhs->y * rhs->y) + (lhs->z * rhs->z);
}

void mds_vector_mulvec( mds_vector *tgt, const Real u, const mds_vector *from, const mds_vector *to)
{
	assert( tgt );
	assert( from );
	assert( to );
	
	tgt->x = u * (to->x - from->x);
	tgt->y = u * (to->y - from->y);
	tgt->z = u * (to->z - from->z);
	
}

void mds_vector_cpy( mds_vector *lhs, const mds_vector *rhs )
{
	assert( lhs );
	assert( rhs );
	lhs->x = rhs->x;
	lhs->y = rhs->y;
	lhs->z = rhs->z;
}


#include "./sys/rand.h"
void mds_vector_rand( mds_vector *tgt, const mds_vector *box )
{
	assert( tgt );
	assert( box );
	tgt->x = mds_alea() * box->x;
	tgt->y = mds_alea() * box->y;
	tgt->z = mds_alea() * box->z;
}

void mds_vector_unit( mds_vector *tgt )
{
	static const Real one = 1;
	assert(tgt);
	Real x1,x1sq,x2,x2sq,sumsq,sqrta,sqrtv;
	do
	{
		x1    = mds_alea();
		x2    = mds_alea();
		x1sq  = x1 * x1;
		x2sq  = x2 * x2;
		sumsq = x1sq+x2sq; 
		sqrta = one - sumsq;
	} while( sqrta < 0 );
	sqrtv = (Real)sqrt( sqrta );
	x1 *= sqrtv;
	x2 *= sqrtv;
	tgt->x = x1+x1;
	tgt->y = x2+x2;
	tgt->z = sqrta - sumsq;
}

#include "./sys/string.h"


int mds_box_parse( mds_vector *box, const char *fmt )
{
	static const char delim[] = " ,";
	assert( box );
	assert( fmt );
	{
		char  *input = mds_string_copy( fmt );
		char  *ibrk  = NULL;
		char  *word  = NULL;
		size_t indx  = 0;
		int    ans   = 0;
		Real  *arr   = & box->x;
		if( !input )
			return -1;
		
		--arr;
		for(word = mds_strtok_r(input, delim, & ibrk); 
			word;
			word = mds_strtok_r(NULL,  delim, & ibrk) )
		{
			++indx;
			if( indx > 3 )
			{
				errno = EINVAL;
				ans   = -1;
				goto END;
			}
			else 
			{
				const Real u = (Real)strtod(word, NULL);
				if( u <= 0 )
				{
					errno = EDOM;
					ans   = -1;
					goto END;
				}
				arr[indx] = u;
			}
			
		}
		
		switch( indx )
		{
			case 1:
				assert( box->x > 0 );
				box->y = box->z = box->x;
				break;
				
			case 3:
				break;
				
			case 2:
			default:
				errno = EINVAL;
				ans   = -1;
				break;
		}
		
		
	END:
		mds_string_delete(input);
		return ans;
		
	}
}





